#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"
#include "upros_message/ArmPosition.h"
#include <move_base_msgs/MoveBaseAction.h>
#include "std_srvs/Empty.h"
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

void sleep(double second)
{
    ros::Duration(second).sleep();
}
int x;
int y;
int z;
int main(int argc, char **argv)
{

    ros::init(argc, argv, "mgrab_test");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle nh;
    // 使用到的 service
    ros::ServiceClient arm_move_open_client = nh.serviceClient<upros_message::ArmPosition>("/upros_arm_control/arm_pos_service_open");
    ros::ServiceClient arm_zero_client = nh.serviceClient<std_srvs::Empty>("/upros_arm_control/zero_service");
    ros::ServiceClient arm_grab_client = nh.serviceClient<std_srvs::Empty>("/upros_arm_control/grab_service");
    ros::ServiceClient arm_release_client = nh.serviceClient<std_srvs::Empty>("/upros_arm_control/release_service");
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);  
    geometry_msgs::Twist vel_msg;

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    ROS_INFO("tf coordinate transformaing....");
    int max_attempts = 2;
    for (int attempt = 0; attempt < max_attempts && ros::ok(); attempt++) {
        try {
    // 获取tag到机械臂基坐标的坐标变换
            geometry_msgs::TransformStamped tfs_1 = buffer.lookupTransform("arm_base_link", "tag_1", ros::Time(0), ros::Duration(1.0));

    // 单位转换，ros坐标系到逆运算坐标系
            x = -int(tfs_1.transform.translation.y * 1000);
            y = int(tfs_1.transform.translation.x * 1000) + 30;
            z = int(tfs_1.transform.translation.z * 1000 + 40);
    
            ROS_INFO("Target X: %d, Target Y: %d, Target Z: %d", x, y, z);
            std_srvs::Empty empty_srv;
    if (abs(y) > 292 && abs(y) <320) {
        ros::Rate loop_rate(10);
        vel_msg.linear.x = 0.1;
        pub.publish(vel_msg);
        int count = 0;
        while (ros::ok() && count < 6)
        {
            pub.publish(vel_msg);
            ros::spinOnce();
            loop_rate.sleep();
            count++;
        }
        // 停下
        vel_msg.linear.x = 0.0;
        pub.publish(vel_msg);
        sleep(1.0);
        geometry_msgs::TransformStamped tfs_2 = buffer.lookupTransform("arm_base_link", "tag_1", ros::Time(0), ros::Duration(1.0));

    // 单位转换，ros坐标系到逆运算坐标系
            x = -int(tfs_2.transform.translation.y * 1000);
            y = int(tfs_2.transform.translation.x * 1000) + 30;
            z = int(tfs_2.transform.translation.z * 1000 + 40);
    }
    if (abs(y) > 320) {
        ros::Rate loop_rate(10);
        vel_msg.linear.x = 0.1;
        pub.publish(vel_msg);
        int count = 0;
        while (ros::ok() && count < 8)
        {
            pub.publish(vel_msg);
            ros::spinOnce();
            loop_rate.sleep();
            count++;
        }
        // 停下
        vel_msg.linear.x = 0.0;
        pub.publish(vel_msg);
        sleep(1.0);
        geometry_msgs::TransformStamped tfs_3 = buffer.lookupTransform("arm_base_link", "tag_1", ros::Time(0), ros::Duration(1.0));

    // 单位转换，ros坐标系到逆运算坐标系
            x = -int(tfs_3.transform.translation.y * 1000);
            y = int(tfs_3.transform.translation.x * 1000) + 30;
            z = int(tfs_3.transform.translation.z * 1000 + 40);
    }
    // 第一步，打开夹爪
    // arm_release_client.call(empty_srv);
    // sleep(0.5);

    // 第二步，运动到抓取位置
    upros_message::ArmPosition move_srv;
    move_srv.request.x = x;
    move_srv.request.y = y+10;//+9
    move_srv.request.z = z-17;//-5
    arm_move_open_client.call(move_srv);
    sleep(0.8);


    // 第三步，闭合夹爪
    arm_grab_client.call(empty_srv);
    sleep(1.5);

    // 第四步，返回零位
    arm_zero_client.call(empty_srv);
    // sleep(0.5);

    ros::shutdown();
    break; // 成功则退出循环
     }
     catch (tf2::TransformException &ex) {
        ROS_WARN("Attempt %d failed: %s", attempt + 1, ex.what());
        if (attempt < max_attempts - 1) {
             try {
         geometry_msgs::TransformStamped transform = buffer.lookupTransform("map", "base_link", ros::Time(0), ros::Duration(0.5));
            // 获取机器人当前Y坐标
            double robot_y = transform.transform.translation.y;
            const double target_y1=2.28;
            const double target_y2=0.27;
            // 简单比较Y坐标
            if (robot_y > target_y1&&robot_y >1.50||robot_y > target_y2&&robot_y <1.50) {  // 添加小阈值避免抖动
            vel_msg.angular.z = -0.4;//0,9
            int count = 0;
            ros::Rate loop_rate(10);
            while (ros::ok() && count < 3)//13
                {
                pub.publish(vel_msg);
                ros::spinOnce();
                loop_rate.sleep();
                count++;
                    }
                vel_msg.angular.z = 0.0;
;  // 机器人在目标点右侧(Y坐标更大)
            } else  {
            vel_msg.angular.z = 0.4;//0,9
            int count = 0;
            ros::Rate loop_rate(10);
            while (ros::ok() && count < 3)//13
                {
                pub.publish(vel_msg);
                ros::spinOnce();
                loop_rate.sleep();
                count++;
                    }
                vel_msg.angular.z = 0.0;; // 机器人在目标点左侧(Y坐标更小)
            }
            sleep(0.5);
            }catch (tf2::TransformException &ex2) {
                    ROS_ERROR("Position check failed: %s", ex2.what());
                }

        }else {
            return -1; // 返回非0表示异常退出
        }
    }
}
    ros::shutdown();
    return 0;
}
